﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Runtime.InteropServices;
using System.IO;
using Microsoft.Kinect;

namespace jKinect.Tools
{
    public static class SkeletonExtensions
    {
        //public static Vector3 ToVector3D(this Vector vector)
        //{
        //    return new Vector3(vector.X, vector.Y, vector.Z);
        //}

        //public static Vector3 ToVector3D(this Vector vector, bool rescale)
        //{
        //    if (rescale)
        //        return new Vector3(vector.X, vector.Y, vector.Z) * 150f + new Vector3(300f, 300f, 300f);
        //    else
        //        return vector.ToVector3D();
        //}

        //public static Vector ToKinectVector(this Vector3 vector)
        //{
        //    return new Vector() { X = vector.X, Y = vector.Y, Z = vector.Z };
        //}

        public static SkeletonPoint ScaleTo(this SkeletonPoint position, float maxPixelWidth, float maxPixelHeight, float originalAmplitudeX, float originalAmplitudeY)
        {
            return new SkeletonPoint
            {
                X = Scale(maxPixelWidth, originalAmplitudeX, position.X),
                Y = Scale(maxPixelHeight, originalAmplitudeY, position.Y),
                Z = position.Z
            };
        }
        public static SkeletonPoint ScaleTo(this SkeletonPoint position, KinectManager kinectManager, float maxPixelWidth, float maxPixelHeight)
        {
            ColorImagePoint point = kinectManager.Runtime.CoordinateMapper.MapSkeletonPointToColorPoint(position, ColorImageFormat.RgbResolution640x480Fps30);
            return new SkeletonPoint() { X = point.X, Y = point.Y, Z = position.Z }.ScaleTo(maxPixelWidth, maxPixelHeight, 640f, 480f);
        }

        public static SkeletonPoint CenteredScaleTo(this SkeletonPoint position, SkeletonPoint refPosition, KinectManager kinectManager, float maxPixelWidth, float maxPixelHeight)
        {
            ColorImagePoint point = kinectManager.Runtime.CoordinateMapper.MapSkeletonPointToColorPoint(position, ColorImageFormat.RgbResolution640x480Fps30);
            ColorImagePoint refPoint = kinectManager.Runtime.CoordinateMapper.MapSkeletonPointToColorPoint(refPosition, ColorImageFormat.RgbResolution640x480Fps30);
            return new SkeletonPoint() { X = point.X - refPoint.X, Y = point.Y - refPoint.Y, Z = (position.Z - refPosition.Z)/3}.ScaleTo(maxPixelWidth, maxPixelHeight, 640f, 480f);
        }

        private static float Scale(float maxPixel, float originalAmplitude, float position)
        {
            float num = (float)maxPixel / originalAmplitude / 2f * position + (float)(maxPixel / 2);
            if (num > (float)maxPixel)
            {
                return (float)maxPixel;
            }
            if (num < 0f)
            {
                return 0f;
            }
            return num;
        }
    }
}
